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Abstract — We introduce the framework of cooperative simul- 
taneous localization and tracking (CoSLAT), which provides a 
consistent combination of cooperative self-localization (CSL) and 
distributed target tracking (DTT) in sensor networks without 
a fusion center. CoSLAT extends simultaneous localization and 
tracking (SLAT) in that it uses also intersensor measurements. 
Starting from a factor graph formulation of the CoSLAT prob- 
lem, we develop a particle-based, distributed message passing 
algorithm for CoSLAT that combines nonparametric belief prop- 
agation with the likelihood consensus scheme. The proposed 
CoSLAT algorithm improves on state-of-the-art CSL and DTT 
algorithms by exchanging probabilistic information between CSL 
and DTT. Simulation results demonstrate substantial improve- 
ments in both self-localization and tracking performance. 

Index Terms — Distributed target tracking, cooperative local- 
ization, CoSLAT, nonparametric belief propagation, likelihood 
consensus. 

I. Introduction 

Two important inference tasks in decentralized sensor net- 
works are cooperative self-localization (CSL) [fl~), (2) and 
distributed target tracking (DTT) 0. In CSL, each sensor 
acquires measurements of its own location relative to neigh- 
boring sensors, and it cooperates with all the other sensors 
to estimate its own location. Existing CSL algorithms include 
nonparametric belief propagation (NBP) (4) and other message 
passing algorithms J2], 0. In DTT, each sensor acquires a 
measurement that is related to the state of a target, and it coop- 
eratively estimates the target state based on the measurements 
of all sensors. Existing DTT algorithms include consensus- 
based distributed particle filters ||6]-|[8]. In the framework 
of distributed simultaneous localization and tracking (SLAT) 
J9], the sensors simultaneously track a target and localize 
themselves, however without using intersensor distance mea- 
surements. Methods for SLAT were proposed in ll9ll- lfT4ll . 

CSL and DTT are closely related since (i) to contribute to 
DTT, a sensor needs to have information of its own location, 
and (ii) the accuracy of CSL may be improved if the sensors 
possess estimates of the state of a target. This observation 
motivates the development of combined CSL-DTT methods. 

Here, we introduce the framework of cooperative simulta- 
neous localization and tracking (CoSLAT), which, for the first 
time, provides a consistent combination of CSL and DTT. 
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SI 0603 (Statistical Inference) within the National Research Network SISE and 
by the WWTF under Award ICT 10-066 (NOWIRE). 



CoSLAT extends SLAT in that it uses also intersensor dis- 
tance measurements. We propose a particle-based, distributed 
CoSLAT algorithm that integrates DTT in NBP-based CSL 
J3, flU, lfT31 . A fundamental problem — the nonavailability of 
essential information at the sensors — is solved by using the 
likelihood consensus (LC) scheme (6], Ifl6l . The algorithm's 
main new feature is a probabilistic information transfer be- 
tween CSL and DTT, which allows CSL and DTT to support 
each other. As we will demonstrate, this leads to improved 
performance of both sensor localization and target tracking. 

This paper is organized as follows. The system model is 
described in Section [II] In Section [HI] the CoSLAT problem 
is defined and a basic message passing scheme for CoSLAT 
is derived. This scheme is further developed into a distributed 
CoSLAT algorithm in Section [IV] Finally, simulation results 
are presented in Section [V] 

II. System Model 

We consider a sensor network consisting of K cooperating 
sensor nodes and a noncooperative target node, as depicted 
in Fig. Q] The set of all nodes is A = {0, . . . , K}, with 
k = indexing the target and k G „4^o = -4\{0} indexing 
the sensors. Sensors and target may be mobile. The state of 
sensor or target k G A at time n G {0,1,...}, denoted by 
Xfc n , consists of the current location and, possibly, additional 
motion parameters such as velocity 1171 . The states Xfc n 
evolve according to the state transition probability density 
functions (pdfs) /(xfc jn |xfc in _i) and the state priors /(x^.o). 

The communication and measurement topologies are de- 
scribed by sets C n , Aik,n< ar, d T n as follows. Two sensors 
k, I G A^o are able to communicate with each other if 
(k, I) G C„ Q A^o x A~o- Cn is symmetric, i.e., if (k, I) G C„ 
then (I, k) G C n . Sensor k G A~o acquires a measurement yk,i-, n 
relative to sensor I G A^o, with (k,l) G C n , if I G Mk, n Q 



target 
- communication link 
• measurement link 



Fig. 1. Wireless sensor network with target, communication links, and mea- 
surement links. 




_4\{fc}. Sensor k G ^l^o acquires a measurement yk,o- n relative 
to the target, i.e., € Mk, n , if k £ T n Q -4~o; i.e., T n — 
{k G ^l^o 1 G Mk, n }- The sets C n , Mk, n , and 7^ may be 
time-dependent. An example of communication and mea- 
surement topologies is given in Fig. [TJ We consider a two- 
dimensional (2D) scenario and noisy distance measurements 



Vk,ly 



VkA V 



(1) 



where Xfe „ = [xi t k,n X2.k.n] T represents the location of sensor 
or target k (note that this a part of the state Xfc „). The 
measurement noise Vk,i-, n is not necessarily Gaussian; its 
variance crj is assumed known; and Vk.i-. n and Vk',i' ;n ' are 
assumed independent unless (k, l,ri) = (k 1 , 1', n'). We note that 
other measurement models could be used, and the extension 
to the 3D case is straightforward. 

III. A Message Passing Scheme 

We first define the CoSLAT problem and derive a message 
passing scheme for CoSLAT. This scheme will be developed 
into a distributed CoSLAT algorithm in Section [TV] 

In CoSLAT, at time n, each sensor k G A^o estimates both 
its own state Xfc „ and the target state Xo.„, using all the inter- 
sensor and sensor-target distance measurements up to time n, 

i.e., y X ; n = {Vfc,l^'} Jfc6> u ,i6Af JbiB , 1 n'6{i,... ) n}- In Particular, 
the minimum mean square error (MMSE) estimator lfT8l of 
state Xfe.n is given by 



-MMSE 



= E{xfe, n |^i :n } = [xk,nf(*k,n\yi:n)dXk,n, (2) 

for all fcGA Compared to "pure CSL" 0, g), 0, 03] and 
"pure DTT" [6|-[8|, the measurement set 3^i:n is extended in 
that it includes also the respective other measurements (i.e., 
sensor-target distance measurements for the sensor state esti- 
mates x^ SE , k G A^o and intersensor distance measurements 
for the target state estimate xJ^^ SE ). 

The marginal posterior pdf f(x-k,n\yi-.n) involved in (0 
can be calculated by marginalization of the joint posterior pdf 
f (Xo: n \yi:n) of the past and present states of all sensors and 
the target, X -.n = { x fc.n'},,. e ^„, e{0j „ } - B y usin S Ba yes' 
rule and common assumptions (2J, one can show that this 
joint posterior pdf factorizes as follows: 



f(X 0: „\yi:n) OC 



J n'=l 



| /(Xfe,o) | /(Xfe',„' |x fc ',„/_l) 



k'eA 



X fe / n /,X; . 



l£M k 



(3) 



Calculating /(xfc „|3^i :n ) by straightforward marginalization is 
infeasible. However, an approximation of the marginal poste- 
rior, frfc, n (x fc ,„) w f(xk,n\yi:n), can be obtained by executing 
iterative belief propagation message passing Ifl9l on the factor 
graph corresponding to the factorization (0, which is shown 
in Fig. [2 At each time n, P message passing iterations are 
performed. Extending the belief propagation message passing 
scheme for distributed CSL proposed in |2] to include a non- 
cooperative target, the iterated approximate marginal posterior 




Fig. 2. Factor graph for CoSLAT, with sensors k S {1,...,K} and a 
target (k £ 0). We use the short notation = fi'X-k.n' l x fc n'-i) ar, d 
fk,l = /(2/M;n'l x fc,n" x i."')> for n ' e {li -••>»»}. The upper (black) 
dotted boxes correspond to the CSL part; the bottom (red) dotted box 
corresponds to the DTT part. All time indices are omitted for simplicity. Only 
the messages and approximate marginal posteriors involved in calculating 
6l 1 »(xi,n) and 6o,n(xo,n) are shown. Edges between black dotted boxes 
imply communication. 



(AMP) of sensor or target node k € A at message passing 



iteration p, bjjp „(xfc )n ), is obtained as 



i(xfc,n) II ^Sfc(xfc,„), keA~ 

i( x 0,n) EI TO^ (x ,„), fc = 0, 



(4) 



with the "prediction message" 



m_).„(x fei „) = / /(x fei „|x feiT[ _x)6^. i 2_ 1 (x fein _i)dx fc)n _i 



and the "measurement messages" 



(5) 



ff(Vk,i )t 1) (x i ,„) 

keA~o, leA^a 

J /(yfc,0;n|xfc,n,Xo,n)no^fc ( X 0,n) rfx 0,r» , 

k£A~ , 1 = 

J /(2/!,0;n|xo, ra , X^ n ) flf^' (x; >n ) dX-l, n , 

fc = 0, leA^o, 
(6) 



where (x; jn ) and n^_J) (xo ; „) (constituting the "extrin- 

sic information") are given by 



fc'eA4,,„\{o} 

n^^xo.n) = m^„(xo,„) (x ,„) . 

fe'er„\{fe} 



(7) 



However, in the proposed CoSLAT algorithm, we modify 
(0 in that we approximate the extrinsic information by the 
corresponding AMP. This leads to the following approximation 
for the measurement messages: 



If(Vk,l;r 



Ap) 
H->k 



(xz, 



J7(w 

,n J u ] „ 



(xi,„) dx ijn , 
fc = 0. 



(8) 

for all ieA In this way, the costly calculation of the extrinsic 
information (jTJ is avoided. Numerical analysis showed that 
although this approximation leads to slightly overconfident 
AMPs, the estimation performance is not affected. 

Because according to (Q}, Uk,i;n depends only on the lo- 



cations of (sensor or target) nodes k and I, 



Ap) 



(Xfe,n) 



is 2D regardless of the dimension of Xfc. n . The messages 

and AMPs needed for calculating &i „(xx, n ) ar, d &on( x o,n) 
according to (O, and ([8]l are depicted in Fig. [2] Messages 
are sent only forward in time, and iterative message passing 
is performed at each time step individually J2J- We do not 
send messages backward in time because this would cause 
the computation, communication, and memory requirements 
as well as the latency to grow linearly with time. As a 
consequence, m_».„(xfc in ) in (O remains unchanged during 
the message passing iterations. 

The computation of the AMPs b^\{x.k,n) according to (|4]i 
differs from pure CSL and pure DTT For k = (target), the 
local likelihood functions used in DTT (6) are replaced by 
the measurement messages ||8). In this way, the uncertainties 
about the locations of all sensors involved in DTT, k' e T n> 
are taken into account. For k G T n (a sensor involved in 
DTT), also messages from the target node are considered, i.e., 
probabilistic information about the target location is used by 
the sensors for improved self-localization. This probabilistic 
information transfer between the CSL and DTT parts is key 
to the superior performance of CoSLAT. 

IV. A Distributed CoSLAT Algorithm 

Next, we develop the message passing scheme ©, (0, and 
(0 into a distributed CoSLAT algorithm. 

A. Nonparametric Belief Propagation 

Because direct calculation of @, (|5j, and ((H) is still 
infeasible, we use an approximate implementation via NBP 
JU, 03]. In NBP, all AMPs and messages are represented 
by particles x^' and weig hts w [j \ for j e {I,..., J}. This 
particle representation is also suited to multimodal AMPs and 



messages. NBP can be viewed as an extension of particle 
filtering to factor graphs with loops. In a CSL scenario, it 
exhibits fast convergence and high accuracy [2|. An algorith- 
mic description of NBP for CSL can be found in |4), lfT31 : 
the extension to our CoSLAT setting is straightforward. In the 
CoSLAT message passing scheme, all particles representing a 
message have equal weights, i.e., = 1/ J. 

In addition to the particle representation of messages, NBP 
uses an approximate kernel representation that can be easily 
derived from the particle representation. This kernel represen- 
tation provides a closed-form expression that can be evaluated 
at any given point. This is necessary for performing the 
message multiplication in (|4]i and for using the LC (see Section 
HV-Bb . Given a set of particles and weig hts {(xW, W W)} =1 
representing a measurement message m(x), the kernel repre- 
sentation of m (x) is obtained as 



m(x) 



J 

E 



w (3) iY(x-x (3) ), 



(9) 



where, as before, the 2D vector x denotes the location part 
of the state x. A standard choice for the kernel K(x) in the 
2D localization scenario is the 2D Gaussian function -ftT(x) = 
(27tct|-) _1 exp(— ||x|| 2 /(2cr| r )). The variance is usually 
estimated from the particles and weights. When crj- is large, 
to(x) is smooth but some of the finer details of m(x) may 
be smoothed out; when o\ is small, m(x) preserves more of 
these fine details but may exhibit some artificial structure not 
present in m(x) lfT31 . 11201 . 

B. Likelihood Consensus Based Computation o/6q P ^(xo jT1 ) 

In CSL, the NBP message passing scheme can be performed 
in a distributed manner using only local intersensor com- 
munications. With CoSLAT, a distributed implementation is 
complicated by the fact that the target node is noncooperative 
and therefore some vital information is not communicated to 
the sensors. More specifically, calculating the AMP of the 
target state, &q P ^(xo.„), according to (|4]i requires the product 
of measurement messages \ 



ieT w[^ (xo.„). Unfortunately, 
this message product is not available at the sensors. 

We solve this problem by using the LC scheme, which 
was proposed in a different context in JSJ. Consider a sensor 
I £Tn and the kernel approximation to|!^o( x o ! ™) ( see ®) of 
the measurement message m^ (xo.„), which was calculated 
at sensor /. Following the LC principle, the logarithm of 



(xo jTl ) is approximated by a finite-order basis expansion: 



log m ; ( ^ (x 0! „) PS ^0%l >r (yifi-,n)<Pr(xo,n) 



(10) 



r=l 



Here, the basis functions yv(xo in ) do not depend on I, i.e., 
the same set of basis functions is used by all sensors. The 
expansion coefficients r (y^ 0; „), r e {1, ...,R} can be 
calculated locally at sensor I by least squares fitting using the 
particles of the prediction message m_> n (x;. n ) as reference 
points (cf. J6l). Furthermore, we formally set /3j.„ r (yi,o;n) = 
foraU r e {!,..., R} if l<£T n . 



The local approximations ( TTOb entail the following approx- 
imation of the desired message product: 

I] ™I (P !o( X 0.n) ~ II eX P ^^nAyifi-,n)M X 0,n)\ 
l<£T n lET„ \r=l / 

= exp^BW^ r (x ,n)J , (ID 

with 

B%> = E/&(R0;n) = E ^iM&n) , (12) 

where the last equation follows because r (yi,o- n ) = for 
all ItfiTn- The coefficients £?i P r in ( fT2b can be computed at 
each sensor by running R parallel instances of an aver- 
age consensus algorithm or a gossip algorithm ETI . l22l . 
This requires only local communications between neighbor- 
ing sensors. After convergence of the consensus or gos- 
sip algorithms, an approximation of the functional form of 
ri;er r "-z^o( x o.?i) is available at each sensor. Each sen- 
sor is then able to calculate a particle representation of 

ro_>„(x ,n) riier„^So( x o,ri) ~ ko P n( x o,n) (see ©) based 
on the importance sampling principle 0231 . More specifically, 
weights {?%.«}_,•— i associated with the particles {x^}^. _ t 
representing m^„(xo, n ) are obtained by evaluating the ap- 
proximation (fTTT i of Ilier r " - l^o( x °i ra ) at me x oli> i- e -' 
calculating w^ n = expfef =1 B$ r ¥v(x^)J for all j e 
{1, . . . , J}. Then, a resampling step 11231 is performed to 
obtain equally weighted particles representing feQ P ^(xo. n ). 

Once a particle approximation of 6q P ^(xo.„) is available at 
each sensor, computations in the CSL part of the factor graph 
(cf. the upper dotted boxes in Fig. [2]i at message passing iter- 
ation p + 1 can be performed in a distributed way using NBP 
as described in |4), 03). Thus, each sensor k £ A^q is able 
to calculate approximate marginals of its own state Xfc.„ and 
of the target state Xo.„ by means of the NBP implementation 
of dUi, (0, and dH), using information that is either locally 
available or obtained through local communication. 

V. Simulation Results 

We consider a network of K = 7 sensors, of which four are 
mobile sensors and three are anchors (i.e., static sensors with 
perfect location information modeled via Dirac-shaped priors). 
The sensors are placed within a field of size 50 x 50. Each 
sensor has a communication range of 56 and localizes itself 
and the target. We consider two scenarios. In scenario 2, which 
is shown in Fig. [3] the upper-right and lower-left sensors have a 
measurement radius of 20, and therefore, initially (at time n = 
0), they do not have enough partners for self-localization. With 
conventional CSL, at n = 0, these sensors have a multimodal 
marginal posterior and are thus unable to localize themselves. 
The measurement regions of the other five sensors cover the 
entire field. Scenario 1 differs from scenario 2 in that also the 
lower-left sensor covers the entire field. 

The states of the mobile sensors and the target consist of lo- 
cation and velocity, i.e., x fc ,„ = [x lik , n x 2 ,k,n ii,k,n i 2 ,fc,n] T - 
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Fig. 3. Network topology used for the simulations, along with a realization of 
the target and sensor trajectories. Initial mobile sensor locations are indicated 
by crosses, anchor locations by circles, and the initial target location by a star. 
The big dashed circles indicate the measurement regions of the upper-right 
sensor (in both scenarios) and of the lower-left sensor (in scenario 2). 

Mobile sensor trajectories are created by using a Dirac- 
shaped location prior at the locations indicated in Fig. [3] 
However, in the simulations of the algorithms, all mobile 
sensors have a location prior that is uniform on [—500, 500] x 
[—500, 500]. Furthermore, we used a Gaussian sensor velocity 
prior with mean fik,a — [—0.1 — 0.1] T and covariance matrix 
Cfc,o = diag{0.1, 0.1} and a Gaussian target state prior with 
mean jUo,o = [0 5 0.4 0.4] T and covariance matrix Co,o = 
diagjl, 1, 0.001, 0.001}. The mobile sensors and the target 
evolve independently according to Xfc.„ = Gxfc >n _i iWu^, 
n=l, 2, . . . Q3, where the matrices G GK 4x 4 and WgIR 4 * 2 
are chosen as in [6j and the driving noise vectors Ufc „ GR 2 are 
Gaussian, i.e., Uk, n ~ A/"(0, cr^I), with variance a\ =0.0005 
and with Uk, n and Uk', n ' independent unless (k, n) = (k\ n'). 
We performed 500 simulation runs. In each run, the sensors 
and the target move along the specific trajectory realizations 
shown in Fig. [3] The observation noise variance is a 2 v = 2. 
Each mobile sensor starts moving only when it is sufficiently 
localized in the sense that the sum of its estimated location 
coordinate variances is below 5<r 2 . 

We compare the performance of the proposed CoSLAT 
algorithm with that of a state-of-the-art reference method, 
which separately performs CSL by means of NBP as described 
in tTT31 and DTT by means of the LC-based distributed particle 
filter presented in JSJ. The DTT method uses the sensor 
location estimates provided by the CSL method. In both the 
CoSLAT method and the reference method, the LC scheme 
uses an average consensus [21] with five iterations, and the 
basis expansion is a third-order polynomial approximation 
J6], resulting in an expansion order of R = 16. The NBP 
scheme performs P = 3 message passing iterations. The kernel 
variance for the measurement messages (cf. Q) is chosen as 
°K = a v> as recommended in [4 |. The number of particles used 
by both NBP and the distributed particle filter is J = 500. 

Fig. |4] shows the simulated root-mean-square self-localiza- 
tion and target localization errors for n = 0, ... ,75. These 
errors were determined by averaging over all sensors and 
all simulation runs. In scenario 1, for n > 43, the self- 
localization error of CoSLAT is seen to be significantly smaller 
than that of the reference method. This is because with pure 
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Fig. 4. Average root-mean-square errors (RMSE) of sensor self-localization and target tracking versus time n, for (a) scenario 1 and (b) scenario 2. 



CSL, the upper-right sensor has not enough partners for self- 
localization, whereas with CoSLAT, for n > 43, the upper-right 
sensor can use the measurement of its distance to the target to 
calculate the message from the target node, m^ki'X-k.n), and 
use this additional information to improve its self-localization 
performance. The tracking performance of CoSLAT in scenar- 
io 1 is similar to that of the reference method. 

In scenario 2, for n > 43, the self-localization error of 
CoSLAT is again much smaller than that of the reference 
method. In addition, it is also smaller for n < 22. This is 
because in scenario 2, for n < 22, also the lower-left sensor 
has not enough partners for self-localization when pure CSL 
is used. Furthermore, the target tracking error of CoSLAT is 
now significantly smaller than that of the reference method for 
almost all times. This is because with separate CSL and DTT, 
the poor self-localization of the lower-left sensor at n < 22 
degrades the target tracking performance. This higher target 
tracking error is retained for n > 22 even when all sensors 
involved in the target tracking are well localized. 

VI. Conclusion 

The novel framework of cooperative simultaneous localiza- 
tion and tracking (CoSLAT) provides a complete and consis- 
tent combination of cooperative self-localization (CSL) and 
distributed target tracking (DTT). Starting from a factor graph 
formulation of the CoSLAT problem, we developed a particle- 
based, distributed message passing algorithm for CoSLAT 
that performs a probabilistic information transfer between 
CSL and DTT. Simulation results demonstrated significant 
improvements in both self-localization and target tracking 
performance compared to state-of-the-art algorithms. 
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